(* Content-type: application/vnd.wolfram.mathematica *)

(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)

(* CreatedBy='Mathematica 11.0' *)

(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[       158,          7]
NotebookDataLength[    212970,       6001]
NotebookOptionsPosition[    209841,       5890]
NotebookOutlinePosition[    210228,       5907]
CellTagsIndexPosition[    210185,       5904]
WindowFrame->Normal*)

(* Beginning of Notebook Content *)
Notebook[{

Cell[CellGroupData[{
Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T01", "=", 
    RowBox[{"With", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"\[Theta]", "=", 
         SubscriptBox["\[Theta]", "1"]}], ",", 
        RowBox[{"d", "=", "0"}], ",", 
        RowBox[{"a", "=", "0"}], ",", 
        RowBox[{"\[Alpha]", "=", "0"}]}], "}"}], ",", 
      RowBox[{"(", GridBox[{
         {
          RowBox[{"Cos", "[", "\[Theta]", "]"}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Theta]", "]"}]}], "0", "a"},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], "\[Times]", "d"}]},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"Cos", "[", "\[Alpha]", "]"}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Times]", "d"}]},
         {"0", "0", "0", "1"}
        }], ")"}]}], "]"}]}], ")"}], "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.8016959372126937`*^9, 3.801695987440379*^9}, {
  3.801696217462759*^9, 3.8016962199361362`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "1"], "]"}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "1"], "]"}]}], "0", "0"},
     {
      RowBox[{"Sin", "[", 
       SubscriptBox["\[Theta]", "1"], "]"}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "1"], "]"}], "0", "0"},
     {"0", "0", "1", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801695989624493*^9, 3.80169622927316*^9, 
  3.8017039632911625`*^9, 3.8017057516097193`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T12", "=", 
    RowBox[{"With", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"\[Theta]", "=", 
         SubscriptBox["\[Theta]", "2"]}], ",", 
        RowBox[{"d", "=", "0"}], ",", 
        RowBox[{"a", "=", 
         SubscriptBox["a", "1"]}], ",", 
        RowBox[{"\[Alpha]", "=", 
         RowBox[{
          RowBox[{"-", "Pi"}], "/", "2"}]}]}], "}"}], ",", 
      RowBox[{"(", GridBox[{
         {
          RowBox[{"Cos", "[", "\[Theta]", "]"}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Theta]", "]"}]}], "0", "a"},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], "\[Times]", "d"}]},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"Cos", "[", "\[Alpha]", "]"}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Times]", "d"}]},
         {"0", "0", "0", "1"}
        }], ")"}]}], "]"}]}], ")"}], "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.801696041434947*^9, {3.8016962109731092`*^9, 3.8016962137915735`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "2"], "]"}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}]}], "0", 
      SubscriptBox["a", "1"]},
     {"0", "0", "1", "0"},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}]}], 
      RowBox[{"-", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}]}], "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.8016960424891405`*^9, 3.8016962293459635`*^9, 
  3.8017039653367205`*^9, 3.801705751679532*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T23", "=", 
    RowBox[{"With", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"\[Theta]", "=", 
         SubscriptBox["\[Theta]", "3"]}], ",", 
        RowBox[{"d", "=", "0"}], ",", 
        RowBox[{"a", "=", 
         SubscriptBox["a", "2"]}], ",", 
        RowBox[{"\[Alpha]", "=", "0"}]}], "}"}], ",", 
      RowBox[{"(", GridBox[{
         {
          RowBox[{"Cos", "[", "\[Theta]", "]"}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Theta]", "]"}]}], "0", "a"},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], "\[Times]", "d"}]},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"Cos", "[", "\[Alpha]", "]"}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Times]", "d"}]},
         {"0", "0", "0", "1"}
        }], ")"}]}], "]"}]}], ")"}], "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.801696077025701*^9, {3.8016962041633673`*^9, 3.801696207131394*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "3"], "]"}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "3"], "]"}]}], "0", 
      SubscriptBox["a", "2"]},
     {
      RowBox[{"Sin", "[", 
       SubscriptBox["\[Theta]", "3"], "]"}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "3"], "]"}], "0", "0"},
     {"0", "0", "1", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801696078036992*^9, 3.8016962293789053`*^9, 
  3.8017039673433228`*^9, 3.8017057517124996`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T34", "=", 
    RowBox[{"With", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"\[Theta]", "=", 
         SubscriptBox["\[Theta]", "4"]}], ",", 
        RowBox[{"d", "=", 
         SubscriptBox["d", "4"]}], ",", 
        RowBox[{"a", "=", 
         SubscriptBox["a", "3"]}], ",", 
        RowBox[{"\[Alpha]", "=", 
         RowBox[{
          RowBox[{"-", "Pi"}], "/", "2"}]}]}], "}"}], ",", 
      RowBox[{"(", GridBox[{
         {
          RowBox[{"Cos", "[", "\[Theta]", "]"}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Theta]", "]"}]}], "0", "a"},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], "\[Times]", "d"}]},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"Cos", "[", "\[Alpha]", "]"}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Times]", "d"}]},
         {"0", "0", "0", "1"}
        }], ")"}]}], "]"}]}], ")"}], "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.8016960927752924`*^9, {3.8016961975988913`*^9, 3.8016962001371045`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "4"], "]"}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}]}], "0", 
      SubscriptBox["a", "3"]},
     {"0", "0", "1", 
      SubscriptBox["d", "4"]},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}]}], 
      RowBox[{"-", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}]}], "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.8016960936609163`*^9, 3.801696229407798*^9, 
  3.8017039699264402`*^9, 3.801705751744358*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T45", "=", 
    RowBox[{"With", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"\[Theta]", "=", 
         SubscriptBox["\[Theta]", "5"]}], ",", 
        RowBox[{"d", "=", "0"}], ",", 
        RowBox[{"a", "=", "0"}], ",", 
        RowBox[{"\[Alpha]", "=", 
         RowBox[{"Pi", "/", "2"}]}]}], "}"}], ",", 
      RowBox[{"(", GridBox[{
         {
          RowBox[{"Cos", "[", "\[Theta]", "]"}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Theta]", "]"}]}], "0", "a"},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], "\[Times]", "d"}]},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"Cos", "[", "\[Alpha]", "]"}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Times]", "d"}]},
         {"0", "0", "0", "1"}
        }], ")"}]}], "]"}]}], ")"}], "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.801696115128499*^9, {3.8016961845358696`*^9, 3.8016961934340353`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], "0", "0"},
     {"0", "0", 
      RowBox[{"-", "1"}], "0"},
     {
      RowBox[{"Sin", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.80169611603806*^9, 3.801696229434727*^9, 
  3.80170397220133*^9, 3.801705751775276*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T56", "=", 
    RowBox[{"With", "[", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"\[Theta]", "=", 
         SubscriptBox["\[Theta]", "6"]}], ",", 
        RowBox[{"d", "=", "0"}], ",", 
        RowBox[{"a", "=", "0"}], ",", 
        RowBox[{"\[Alpha]", "=", 
         RowBox[{
          RowBox[{"-", "Pi"}], "/", "2"}]}]}], "}"}], ",", 
      RowBox[{"(", GridBox[{
         {
          RowBox[{"Cos", "[", "\[Theta]", "]"}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Theta]", "]"}]}], "0", "a"},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"-", 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], "\[Times]", "d"}]},
         {
          RowBox[{
           RowBox[{"Sin", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Theta]", "]"}], 
           RowBox[{"Sin", "[", "\[Alpha]", "]"}]}], 
          RowBox[{"Cos", "[", "\[Alpha]", "]"}], 
          RowBox[{
           RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Times]", "d"}]},
         {"0", "0", "0", "1"}
        }], ")"}]}], "]"}]}], ")"}], "//", "MatrixForm"}]], "Input",
 CellChangeTimes->{
  3.80169613134715*^9, {3.8016961754820566`*^9, 3.8016961786555605`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "6"], "]"}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], "0", "0"},
     {"0", "0", "1", "0"},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], 
      RowBox[{"-", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801696132347437*^9, 3.801696229462652*^9, 
  3.8017039744193954`*^9, 3.8017057518061924`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["\:8fd0\:52a8\:5b66\:6b63\:89e3",
 FontFamily->"Gill Sans MT",
 FontSize->26]], "Title",
 CellChangeTimes->{3.8017038332678623`*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{"T06", "=", 
  RowBox[{
   RowBox[{
   "T01", ".", "T12", ".", "T23", ".", "T34", ".", "T45", ".", "T56"}], "//", 
   "MatrixForm"}]}]], "Input",
 CellChangeTimes->{
  3.801696162510745*^9, {3.8016962421756535`*^9, 3.8016962520782022`*^9}, 
   3.801703768560951*^9}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "6"], "]"}]}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "3"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["d", "4"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "6"], "]"}]}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "1"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "1"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "3"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["d", "4"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "6"], "]"}]}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "3"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["d", "4"]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801696163864127*^9, 3.801696229527477*^9, 
  3.8017039768040333`*^9, 3.801705751874014*^9}]
}, Open  ]]
}, Open  ]],

Cell[CellGroupData[{

Cell[TextData[StyleBox["\:8fd0\:52a8\:5b66\:9006\:89e3",
 FontFamily->"Gill Sans MT",
 FontSize->26]], "Title",
 CellChangeTimes->{{3.8016963289695187`*^9, 3.8016963431326294`*^9}, {
  3.8017037558040776`*^9, 3.8017037586614313`*^9}}],

Cell[CellGroupData[{

Cell[TextData[StyleBox["1\:3001\:89e3\:6790\:89e3\:ff08pieper\:6cd5\:ff09",
 FontFamily->"Gill Sans MT",
 FontSize->20]], "Chapter",
 CellChangeTimes->{{3.8017036898455577`*^9, 3.801703730889721*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"ITT", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["n", "x"], ",", 
        SubscriptBox["o", "x"], ",", 
        SubscriptBox["a", "x"], ",", 
        SubscriptBox["p", "x"]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["n", "y"], ",", 
        SubscriptBox["o", "y"], ",", 
        SubscriptBox["a", "y"], ",", 
        SubscriptBox["p", "y"]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        SubscriptBox["n", "z"], ",", 
        SubscriptBox["o", "z"], ",", 
        SubscriptBox["a", "z"], ",", 
        SubscriptBox["p", "z"]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ")"}], "//",
   "MatrixForm"}]], "Input"],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      SubscriptBox["n", "x"], 
      SubscriptBox["o", "x"], 
      SubscriptBox["a", "x"], 
      SubscriptBox["p", "x"]},
     {
      SubscriptBox["n", "y"], 
      SubscriptBox["o", "y"], 
      SubscriptBox["a", "y"], 
      SubscriptBox["p", "y"]},
     {
      SubscriptBox["n", "z"], 
      SubscriptBox["o", "z"], 
      SubscriptBox["a", "z"], 
      SubscriptBox["p", "z"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.80170391190063*^9, 3.801703979918684*^9, 
  3.8017057519059258`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"IT01", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], ",", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], ",", "0", ",", "0"}], "}"}], 
      ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], ",", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], ",", "0", ",", "0"}], "}"}], 
      ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ")"}], "//",
   "MatrixForm"}]], "Input"],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "1"], "]"}], 
      RowBox[{"Sin", "[", 
       SubscriptBox["\[Theta]", "1"], "]"}], "0", "0"},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "1"], "]"}]}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "1"], "]"}], "0", "0"},
     {"0", "0", "1", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.8017039227276683`*^9, 3.8017039837643986`*^9, 
  3.801704646765457*^9, 3.801705751936843*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"(", 
    RowBox[{"T16", "=", 
     RowBox[{
      TagBox[
       RowBox[{"(", "\[NoBreak]", GridBox[{
          {
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], 
           RowBox[{"-", 
            RowBox[{"Sin", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], "0", "0"},
          {"0", "0", "1", "0"},
          {
           RowBox[{"-", 
            RowBox[{"Sin", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], 
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], "0", "0"},
          {"0", "0", "0", "1"}
         },
         GridBoxAlignment->{
          "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
           "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
         GridBoxSpacings->{"Columns" -> {
             Offset[0.27999999999999997`], {
              Offset[0.7]}, 
             Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
             Offset[0.2], {
              Offset[0.4]}, 
             Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
       Function[BoxForm`e$, 
        MatrixForm[BoxForm`e$]]], ".", "T23", ".", "T34", ".", "T45", ".", 
      "T56"}]}], ")"}], "//", "MatrixForm"}], "//", "Simplify"}]], "Input",
 CellChangeTimes->{{3.8017053371891127`*^9, 3.8017053418416405`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "6"], "]"}]}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "6"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "6"], "]"}]}]}], ")"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "6"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "6"], "]"}]}]}], ")"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "5"], "]"}]}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["a", "3"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["d", "4"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "5"], "]"}]}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], "0"},
     {
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "6"], "]"}]}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          RowBox[{
           SubscriptBox["\[Theta]", "2"], "+", 
           SubscriptBox["\[Theta]", "3"]}], "]"}]}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
        SubscriptBox["a", "2"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["a", "3"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["d", "4"]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.8017039356790233`*^9, 3.8017039871054935`*^9, 
  3.8017046489905376`*^9, 3.801705343565031*^9, 3.8017057519767685`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"(", 
    RowBox[{
     TagBox[
      RowBox[{"(", "\[NoBreak]", GridBox[{
         {"1", "0", "0", 
          RowBox[{"-", 
           SubscriptBox["a", "1"]}]},
         {"0", "1", "0", "0"},
         {"0", "0", "1", "0"},
         {"0", "0", "0", "1"}
        },
        GridBoxAlignment->{
         "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, 
          "Rows" -> {{Baseline}}, "RowsIndexed" -> {}},
        GridBoxSpacings->{"Columns" -> {
            Offset[0.27999999999999997`], {
             Offset[0.7]}, 
            Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
            Offset[0.2], {
             Offset[0.4]}, 
            Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
      Function[BoxForm`e$, 
       MatrixForm[BoxForm`e$]]], ".", "IT01", ".", "ITT"}], ")"}], "//", 
   "MatrixForm"}], "//", "Simplify"}]], "Input",
 CellChangeTimes->{{3.8017053166560135`*^9, 3.801705324304553*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["n", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["o", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "y"]}]}], 
      RowBox[{
       RowBox[{"-", 
        SubscriptBox["a", "1"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["p", "y"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["n", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["o", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["p", "y"]}]}]},
     {
      SubscriptBox["n", "z"], 
      SubscriptBox["o", "z"], 
      SubscriptBox["a", "z"], 
      SubscriptBox["p", "z"]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{{3.80170462208949*^9, 3.8017046520672736`*^9}, {
   3.8017053255013504`*^9, 3.8017053472242413`*^9}, 3.8017057520126405`*^9}]
}, Open  ]],

Cell[TextData[{
 "\:4ee5\:4e0a\:4e24\:77e9\:9635\:76f8\:7b49\:ff0c\:53ef\:5f97\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["\[Theta]", "1"], "=", 
   RowBox[{
    RowBox[{"Atan2", 
     RowBox[{"(", 
      RowBox[{
       SubscriptBox["p", "y"], ",", 
       SubscriptBox["p", "x"]}], ")"}]}], "-", 
    RowBox[{"Atan2", 
     RowBox[{"(", 
      RowBox[{"0", ",", 
       RowBox[{"\[PlusMinus]", "1"}]}], ")"}]}]}]}]],
  CellChangeTimes->{{3.750562289658607*^9, 3.750562309388607*^9}, {
   3.750562378028607*^9, 3.750562489545607*^9}},
  Background->RGBColor[1, 0.5, 0],
  ExpressionUUID -> "351c5871-1c9f-47ed-ab7d-1c4b7df82399"],
 "\n\:5de6\:53f3\:77e9\:9635\:7684\:ff081\:ff0c4\:ff09\:4e0e\:ff083\:ff0c4\
\:ff09\:76f8\:7b49\:3001\:79fb\:9879\:ff0c\:5f97\n",
 Cell[BoxData[
  RowBox[{
   RowBox[{
    RowBox[{"-", 
     SubscriptBox["a", "1"]}], "+", 
    RowBox[{
     RowBox[{"Cos", "[", 
      SubscriptBox["\[Theta]", "1"], "]"}], " ", 
     SubscriptBox["p", "x"]}], "+", 
    RowBox[{
     RowBox[{"Sin", "[", 
      SubscriptBox["\[Theta]", "1"], "]"}], 
     SubscriptBox["p", "y"]}]}], "=", 
   RowBox[{
    RowBox[{
     SubscriptBox["a", "2"], " ", 
     RowBox[{"Cos", "[", 
      SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
    RowBox[{"d4", " ", 
     RowBox[{"Sin", "[", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "+", 
       SubscriptBox["\[Theta]", "3"]}], "]"}]}], "+", 
    RowBox[{
     SubscriptBox["a", "3"], " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "+", 
       SubscriptBox["\[Theta]", "3"]}], "]"}]}]}]}]],
  CellChangeTimes->{{3.750562662878607*^9, 3.750562870521607*^9}, {
   3.750563717467607*^9, 3.7505637226486073`*^9}, {3.750563801320607*^9, 
   3.750563804062607*^9}, {3.750563861270607*^9, 3.750564119336607*^9}, {
   3.750564359085607*^9, 3.750564558283607*^9}, {3.750564914671607*^9, 
   3.750564922171607*^9}, {3.750565022220607*^9, 3.750565093179607*^9}, {
   3.750565149777607*^9, 3.7505653141346073`*^9}, {3.7505657804656067`*^9, 
   3.750565791872607*^9}, {3.7505658280056067`*^9, 3.750565924072607*^9}},
  ExpressionUUID -> "6a166d7d-d0a6-4ea5-82a4-01263fd89eed"],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["p", "z"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"-", 
      SubscriptBox["a", "2"]}], 
     RowBox[{"Sin", "[", 
      SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
    RowBox[{
     SubscriptBox["a", "3"], 
     RowBox[{"Sin", "[", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "+", 
       SubscriptBox["\[Theta]", "3"]}], "]"}]}], "+", 
    RowBox[{
     SubscriptBox["d", "4"], " ", 
     RowBox[{"Cos", "[", 
      RowBox[{
       SubscriptBox["\[Theta]", "2"], "+", 
       SubscriptBox["\[Theta]", "3"]}], "]"}]}]}]}]],
  CellChangeTimes->{{3.750562662878607*^9, 3.750562870521607*^9}, {
   3.750563717467607*^9, 3.7505637226486073`*^9}, {3.750563801320607*^9, 
   3.750563804062607*^9}, {3.750563861270607*^9, 3.750564119336607*^9}, {
   3.750564359085607*^9, 3.750564558283607*^9}, {3.750564914671607*^9, 
   3.750564922171607*^9}, {3.750565022220607*^9, 3.750565093179607*^9}, {
   3.750565149777607*^9, 3.7505653141346073`*^9}, {3.7505657804656067`*^9, 
   3.750565791872607*^9}, {3.7505658280056067`*^9, 3.750565924072607*^9}},
  ExpressionUUID -> "6a166d7d-d0a6-4ea5-82a4-01263fd89eed"],
 "\n\:5c06\:4ee5\:4e0a\:4e24\:65b9\:7a0b\:5de6\:53f3\:5206\:522b\:5e73\:65b9\
\:ff0c\:7136\:540e\:518d\:76f8\:52a0\:ff0c\:53ef\:5f97\n",
 Cell[BoxData[
  RowBox[{
   SubsuperscriptBox["a", "1", "2"], "+", 
   SubsuperscriptBox["p", "x", "2"], "+", 
   SubsuperscriptBox["p", "y", "2"], "+", 
   SubsuperscriptBox["p", "z", "2"], "-", 
   RowBox[{"2", 
    SubscriptBox["a", "1"], 
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       SubscriptBox["p", "x"], 
       SubscriptBox["c", "1"]}], "+", 
      RowBox[{
       SubscriptBox["p", "y"], 
       SubscriptBox["s", "1"]}]}], ")"}], " "}]}]],
  CellChangeTimes->{{3.750562662878607*^9, 3.750562870521607*^9}, {
   3.750563717467607*^9, 3.7505637226486073`*^9}, {3.750563801320607*^9, 
   3.750563804062607*^9}, {3.750563861270607*^9, 3.750564119336607*^9}, {
   3.750564359085607*^9, 3.750564558283607*^9}, {3.750564914671607*^9, 
   3.750564922171607*^9}, {3.750565022220607*^9, 3.750565093179607*^9}, {
   3.750565149777607*^9, 3.7505653141346073`*^9}, {3.7505657804656067`*^9, 
   3.750565791872607*^9}, {3.7505658280056067`*^9, 3.750565924072607*^9}},
  ExpressionUUID -> "6a166d7d-d0a6-4ea5-82a4-01263fd89eed"],
 "=  ",
 Cell[BoxData[
  RowBox[{
   SubsuperscriptBox["a", "2", "2"], "+", 
   SubsuperscriptBox["d", "4", "2"], "+", 
   SubsuperscriptBox["a", "3", "2"], "-", 
   RowBox[{"2", 
    SubscriptBox["a", "2"], 
    SubscriptBox["d", "4"], 
    SubscriptBox["s", "3"]}], "+", 
   RowBox[{"2", 
    SubscriptBox["a", "2"], 
    SubscriptBox["a", "3"], 
    SubscriptBox["c", "3"]}]}]],
  CellChangeTimes->{{3.750562662878607*^9, 3.750562870521607*^9}, {
   3.750563717467607*^9, 3.7505637226486073`*^9}, {3.750563801320607*^9, 
   3.750563804062607*^9}, {3.750563861270607*^9, 3.750564119336607*^9}, {
   3.750564359085607*^9, 3.750564558283607*^9}, {3.750564914671607*^9, 
   3.750564922171607*^9}, {3.750565022220607*^9, 3.750565093179607*^9}, {
   3.750565149777607*^9, 3.7505653141346073`*^9}, {3.7505657804656067`*^9, 
   3.750565791872607*^9}, {3.7505658280056067`*^9, 3.750565924072607*^9}},
  ExpressionUUID -> "6a166d7d-d0a6-4ea5-82a4-01263fd89eed"],
 "\n\:4ee4",
 Cell[BoxData[
  RowBox[{
   RowBox[{
    RowBox[{
     SubscriptBox["a", "3"], 
     SubscriptBox["c", "3"]}], "-", 
    RowBox[{
     SubscriptBox["d", "4"], 
     SubscriptBox["s", "3"]}]}], "=", "k"}]],
  CellChangeTimes->{{3.750562662878607*^9, 3.750562870521607*^9}, {
   3.750563717467607*^9, 3.7505637226486073`*^9}, {3.750563801320607*^9, 
   3.750563804062607*^9}, {3.750563861270607*^9, 3.750564119336607*^9}, {
   3.750564359085607*^9, 3.750564558283607*^9}, {3.750564914671607*^9, 
   3.750564922171607*^9}, {3.750565022220607*^9, 3.750565093179607*^9}, {
   3.750565149777607*^9, 3.7505653141346073`*^9}, {3.7505657804656067`*^9, 
   3.750565791872607*^9}, {3.7505658280056067`*^9, 3.750565924072607*^9}},
  ExpressionUUID -> "6a166d7d-d0a6-4ea5-82a4-01263fd89eed"],
 "\:ff0c\:5219\n",
 Cell[BoxData[
  RowBox[{"k", "=", 
   FractionBox[
    RowBox[{
     SubsuperscriptBox["p", "x", "2"], "+", 
     SubsuperscriptBox["p", "y", "2"], "+", 
     SubsuperscriptBox["p", "z", "2"], "+", 
     SubsuperscriptBox["a", "1", "2"], "-", 
     SubsuperscriptBox["a", "2", "2"], "-", 
     SubsuperscriptBox["a", "3", "2"], "-", 
     SubsuperscriptBox["d", "4", "2"], "-", 
     RowBox[{"2", 
      SubscriptBox["a", "1"], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], 
         SubscriptBox["c", "1"]}], "+", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}]}], ")"}]}]}], 
    RowBox[{"2", 
     SubscriptBox["a", "2"]}]]}]],
  CellChangeTimes->{{3.750562662878607*^9, 3.750562870521607*^9}, {
   3.750563717467607*^9, 3.7505637226486073`*^9}, {3.750563801320607*^9, 
   3.750563804062607*^9}, {3.750563861270607*^9, 3.750564119336607*^9}, {
   3.750564359085607*^9, 3.750564558283607*^9}, {3.750564914671607*^9, 
   3.750564922171607*^9}, {3.750565022220607*^9, 3.750565093179607*^9}, {
   3.750565149777607*^9, 3.7505653141346073`*^9}, {3.7505657804656067`*^9, 
   3.750565791872607*^9}, {3.7505658280056067`*^9, 3.750565924072607*^9}},
  ExpressionUUID -> "6a166d7d-d0a6-4ea5-82a4-01263fd89eed"],
 "\n\:56e0\:6b64\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["\[Theta]", "3"], "=", 
   RowBox[{
    RowBox[{"Atan2", 
     RowBox[{"(", 
      RowBox[{
       SubscriptBox["a", "3"], ",", 
       SubscriptBox["d", "4"]}], ")"}]}], "-", 
    RowBox[{"Atan2", 
     RowBox[{"(", 
      RowBox[{"k", ",", 
       RowBox[{"\[PlusMinus]", 
        SqrtBox[
         RowBox[{
          SubsuperscriptBox["a", "3", "2"], "+", 
          SubsuperscriptBox["d", "4", "2"], "-", 
          SuperscriptBox["k", "2"]}]]}]}], ")"}]}]}]}]],
  Background->RGBColor[1, 0.5, 0]]
}], "Text",
 CellChangeTimes->{{3.8017046961483603`*^9, 3.8017047091087236`*^9}, {
   3.8017047517895575`*^9, 3.801704833518897*^9}, {3.801704887666054*^9, 
   3.8017049216262417`*^9}, {3.8017050962407565`*^9, 3.801705221924157*^9}, {
   3.8017053798579473`*^9, 3.8017055314416943`*^9}, {3.801705668731464*^9, 
   3.8017056687334137`*^9}, 3.8017057032131805`*^9, {3.8017057783611937`*^9, 
   3.801705785376394*^9}, {3.8017058597664022`*^9, 3.8017058797838573`*^9}, {
   3.8017059719552994`*^9, 3.801706031415246*^9}, {3.8017064451040597`*^9, 
   3.8017064589140944`*^9}, {3.8017064927944565`*^9, 3.801706499877509*^9}, {
   3.8017065400879507`*^9, 3.8017065797727957`*^9}, {3.8017066136900997`*^9, 
   3.8017066290310316`*^9}, {3.80170669118752*^9, 3.801706700257286*^9}, {
   3.8017067628877254`*^9, 3.801706833195698*^9}, {3.8017079794865227`*^9, 
   3.8017079858615*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"IT12", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], ",", "0", ",", 
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "2"], "]"}]}], ",", 
        RowBox[{
         RowBox[{"-", "a1"}], "*", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "2"], "]"}]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "2"], "]"}]}], ",", "0", ",", 
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "2"], "]"}]}], ",", 
        RowBox[{
         SubscriptBox["a", "1"], "*", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "2"], "]"}]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "1", ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ")"}], "//",
   "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.801706916869827*^9, 3.8017069327672997`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "2"], "]"}], "0", 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}]}], 
      RowBox[{
       RowBox[{"-", "a1"}], " ", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}]}]},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}]}], "0", 
      RowBox[{"-", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}]}], 
      RowBox[{
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "2"], "]"}], " ", 
       SubscriptBox["a", "1"]}]},
     {"0", "1", "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{{3.8017069070471015`*^9, 3.8017069350591726`*^9}}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"IT23", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], ",", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], ",", "0", ",", 
        RowBox[{
         RowBox[{"-", 
          SubscriptBox["a", "2"]}], "*", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "3"], "]"}]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "3"], "]"}]}], ",", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], ",", "0", ",", 
        RowBox[{
         SubscriptBox["a", "2"], "*", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "3"], "]"}]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "1", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ")"}], "//",
   "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.801706957156099*^9, 3.801706972421228*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "3"], "]"}], 
      RowBox[{"Sin", "[", 
       SubscriptBox["\[Theta]", "3"], "]"}], "0", 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
       SubscriptBox["a", "2"]}]},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "3"], "]"}]}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "3"], "]"}], "0", 
      RowBox[{
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "3"], "]"}], " ", 
       SubscriptBox["a", "2"]}]},
     {"0", "0", "1", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801706974677193*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"(", 
    RowBox[{"IT23", ".", "IT12", ".", "IT01", ".", "ITT"}], ")"}], "//", 
   "MatrixForm"}], "//", "Simplify"}]], "Input",
 CellChangeTimes->{{3.8017075117605753`*^9, 3.8017075154128056`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["n", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["o", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", "a1"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["p", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["p", "z"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["n", "x"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["n", "y"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["o", "x"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["o", "y"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["a", "x"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["a", "y"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{"a1", " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["p", "x"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["p", "y"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        SubscriptBox["p", "z"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["n", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["o", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["a", "y"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        SubscriptBox["p", "y"]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801707012085127*^9, 3.801707516835001*^9, 
  3.801707632913495*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T36", "=", 
    RowBox[{"T34", ".", "T45", ".", "T56"}]}], ")"}], "//", 
  "MatrixForm"}]], "Input"],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "6"], "]"}]}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], 
      SubscriptBox["a", "3"]},
     {
      RowBox[{
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], 
      SubscriptBox["d", "4"]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "5"], "]"}]}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "6"], "]"}]}]}], 
      RowBox[{
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801707021606658*^9, 3.8017075204144564`*^9}]
}, Open  ]],

Cell[TextData[{
 "\:4ee5\:4e0a\:4e24\:77e9\:9635\:76f8\:7b49\:ff0c\:5373\:77e9\:9635\:4e24\
\:8fb9\:ff081\:ff0c4\:ff09\:9879\:548c\:ff082\:ff0c4\:ff09\:9879\:76f8\:7b49\
\:ff0c\:56e0\:6b64\n",
 Cell[BoxData[
  RowBox[{
   RowBox[{
    RowBox[{"-", 
     SubscriptBox["a", "1"]}], " ", 
    RowBox[{"Cos", "[", 
     SubscriptBox["\[Theta]", "2"], "]"}], " ", 
    RowBox[{"Cos", "[", 
     SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
   RowBox[{
    RowBox[{"Sin", "[", 
     SubscriptBox["\[Theta]", "2"], "]"}], " ", 
    RowBox[{"Sin", "[", 
     SubscriptBox["\[Theta]", "3"], "]"}], " ", 
    SubscriptBox["a", "1"]}], "-", 
   RowBox[{
    RowBox[{"Cos", "[", 
     SubscriptBox["\[Theta]", "3"], "]"}], " ", 
    SubscriptBox["a", "2"]}], "+", 
   RowBox[{
    RowBox[{"Cos", "[", 
     SubscriptBox["\[Theta]", "1"], "]"}], " ", 
    RowBox[{"Cos", "[", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "+", 
      SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
    SubscriptBox["p", "x"]}], "+", 
   RowBox[{
    RowBox[{"Cos", "[", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "+", 
      SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
    RowBox[{"Sin", "[", 
     SubscriptBox["\[Theta]", "1"], "]"}], " ", 
    SubscriptBox["p", "y"]}], "-", 
   RowBox[{
    RowBox[{"Sin", "[", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "+", 
      SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
    SubscriptBox["p", "z"]}]}]], "Output",
  GeneratedCell->False,
  CellAutoOverwrite->False,
  CellChangeTimes->{3.801707012085127*^9, 3.801707516835001*^9}],
 "=",
 Cell[BoxData[
  SubscriptBox["a", "3"]], "Output",
  GeneratedCell->False,
  CellAutoOverwrite->False,
  CellChangeTimes->{3.801707021606658*^9, 3.8017075204144564`*^9}],
 "\n",
 Cell[BoxData[
  RowBox[{
   RowBox[{
    SubscriptBox["a", "1"], " ", 
    RowBox[{"Cos", "[", 
     SubscriptBox["\[Theta]", "2"], "]"}], " ", 
    RowBox[{"Sin", "[", 
     SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
   RowBox[{
    RowBox[{"Cos", "[", 
     SubscriptBox["\[Theta]", "3"], "]"}], " ", 
    RowBox[{"Sin", "[", 
     SubscriptBox["\[Theta]", "2"], "]"}], " ", 
    SubscriptBox["a", "1"]}], "+", 
   RowBox[{
    RowBox[{"Sin", "[", 
     SubscriptBox["\[Theta]", "3"], "]"}], " ", 
    SubscriptBox["a", "2"]}], "-", 
   RowBox[{
    RowBox[{"Cos", "[", 
     SubscriptBox["\[Theta]", "1"], "]"}], " ", 
    RowBox[{"Sin", "[", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "+", 
      SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
    SubscriptBox["p", "x"]}], "-", 
   RowBox[{
    RowBox[{"Sin", "[", 
     SubscriptBox["\[Theta]", "1"], "]"}], " ", 
    RowBox[{"Sin", "[", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "+", 
      SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
    SubscriptBox["p", "y"]}], "-", 
   RowBox[{
    RowBox[{"Cos", "[", 
     RowBox[{
      SubscriptBox["\[Theta]", "2"], "+", 
      SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
    SubscriptBox["p", "z"]}]}]], "Output",
  GeneratedCell->False,
  CellAutoOverwrite->False,
  CellChangeTimes->{3.801707012085127*^9, 3.801707516835001*^9}],
 "=",
 Cell[BoxData[
  SubscriptBox["d", "4"]], "Output",
  CellChangeTimes->{3.801707021606658*^9, 3.8017075204144564`*^9}],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["s", "23"], "=", 
   FractionBox[
    RowBox[{
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["a", "3"], "+", 
        RowBox[{
         SubscriptBox["a", "2"], 
         SubscriptBox["c", "3"]}]}], ")"}], 
      RowBox[{"(", 
       RowBox[{"-", 
        SubscriptBox["p", "z"]}], ")"}]}], "-", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], 
         SubscriptBox["c", "1"]}], "+", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "-", 
        SubscriptBox["a", "1"]}], ")"}], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", 
          SubscriptBox["p", "x"]}], 
         SubscriptBox["c", "1"]}], "-", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "+", 
        SubscriptBox["a", "1"]}], ")"}]}]}], 
    RowBox[{
     RowBox[{
      RowBox[{"(", 
       RowBox[{"-", 
        SubscriptBox["p", "z"]}], ")"}], 
      RowBox[{"(", 
       RowBox[{"-", 
        SubscriptBox["p", "z"]}], ")"}]}], "-", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], 
         SubscriptBox["c", "1"]}], "+", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "-", 
        SubscriptBox["a", "1"]}], ")"}], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", 
          SubscriptBox["p", "x"]}], 
         SubscriptBox["c", "1"]}], "-", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "+", 
        SubscriptBox["a", "1"]}], ")"}]}]}]]}]], "Input",
  CellChangeTimes->{{3.750569208342607*^9, 3.750569221992607*^9}, {
    3.750569403356607*^9, 3.7505694397406073`*^9}, 3.750588270768607*^9, {
    3.750588304710607*^9, 3.7505885623406067`*^9}, 3.750588619680607*^9, {
    3.7505888339136066`*^9, 3.750589142937607*^9}},
  ExpressionUUID -> "7d1beb80-7da8-4080-a2fe-03635f4af7cf"],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["c", "23"], "=", 
   FractionBox[
    RowBox[{
     RowBox[{
      RowBox[{"(", 
       RowBox[{"-", 
        SubscriptBox["p", "z"]}], ")"}], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", 
          SubscriptBox["p", "x"]}], 
         SubscriptBox["c", "1"]}], "-", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "+", 
        SubscriptBox["a", "1"]}], ")"}]}], "-", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        SubscriptBox["a", "3"], "+", 
        RowBox[{
         SubscriptBox["a", "2"], 
         SubscriptBox["c", "3"]}]}], ")"}], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", 
          SubscriptBox["p", "x"]}], 
         SubscriptBox["c", "1"]}], "-", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "+", 
        SubscriptBox["a", "1"]}], ")"}]}]}], 
    RowBox[{
     RowBox[{
      RowBox[{"(", 
       RowBox[{"-", 
        SubscriptBox["p", "z"]}], ")"}], 
      RowBox[{"(", 
       RowBox[{"-", 
        SubscriptBox["p", "z"]}], ")"}]}], "-", 
     RowBox[{
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         SubscriptBox["p", "x"], 
         SubscriptBox["c", "1"]}], "+", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "-", 
        SubscriptBox["a", "1"]}], ")"}], 
      RowBox[{"(", 
       RowBox[{
        RowBox[{
         RowBox[{"-", 
          SubscriptBox["p", "x"]}], 
         SubscriptBox["c", "1"]}], "-", 
        RowBox[{
         SubscriptBox["p", "y"], 
         SubscriptBox["s", "1"]}], "+", 
        SubscriptBox["a", "1"]}], ")"}]}]}]]}]], "Input",
  CellChangeTimes->{{3.750569208342607*^9, 3.750569221992607*^9}, {
    3.750569403356607*^9, 3.7505694397406073`*^9}, 3.750588270768607*^9, {
    3.750588304710607*^9, 3.7505885623406067`*^9}, 3.750588619680607*^9, {
    3.7505888339136066`*^9, 3.750589142937607*^9}},
  ExpressionUUID -> "7d1beb80-7da8-4080-a2fe-03635f4af7cf"],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["\[Theta]", "2"], "=", 
   RowBox[{
    RowBox[{"Atan2", 
     RowBox[{"(", 
      RowBox[{
       SubscriptBox["s", "23"], ",", 
       SubscriptBox["c", "23"]}], ")"}]}], "-", 
    SubscriptBox["\[Theta]", "3"]}]}]], "Input",
  CellChangeTimes->{{3.750569208342607*^9, 3.750569221992607*^9}, {
    3.750569403356607*^9, 3.7505694397406073`*^9}, 3.750588270768607*^9, {
    3.750588304710607*^9, 3.7505885623406067`*^9}, 3.750588619680607*^9, {
    3.7505888339136066`*^9, 3.750589142937607*^9}},
  Background->RGBColor[1, 0.5, 0],
  ExpressionUUID -> "7d1beb80-7da8-4080-a2fe-03635f4af7cf"],
 "\n\:5f53\:4e0a\:9762\:77e9\:9635\:5f97\:ff082\:ff0c3\:ff09\:5143\:7d20\:4e0d\
\:7b49\:4e8e1\:65f6\:ff0c\n",
 Cell[BoxData[
  RowBox[{
   RowBox[{
    SubscriptBox["s", "4"], 
    SubscriptBox["s", "5"]}], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"-", 
      SubscriptBox["a", "x"]}], 
     SubscriptBox["s", "1"]}], "+", 
    RowBox[{
     SubscriptBox["a", "y"], 
     SubscriptBox["c", "1"]}]}]}]],
  CellChangeTimes->{{3.750590312889607*^9, 3.750590335004607*^9}, {
    3.750590392362607*^9, 3.7505904601096067`*^9}, {3.7506278846588154`*^9, 
    3.750627885860016*^9}, {3.750628035700817*^9, 3.750628035841217*^9}, {
    3.750628161577818*^9, 3.7506281704074173`*^9}, {3.750628216926618*^9, 
    3.7506283115874186`*^9}, {3.750628403207219*^9, 3.7506285135910196`*^9}, 
    3.75062857482102*^9},
  ExpressionUUID -> "34e706c0-fc45-4fee-a126-89599687faec"],
 "\n",
 Cell[BoxData[
  RowBox[{
   RowBox[{
    SubscriptBox["c", "4"], 
    SubscriptBox["s", "5"]}], "=", 
   RowBox[{"-", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       SubscriptBox["a", "x"], 
       SubscriptBox["c", "1"], 
       SubscriptBox["c", "23"]}], "+", 
      RowBox[{
       SubscriptBox["a", "y"], 
       SubscriptBox["s", "y"], 
       SubscriptBox["c", "23"]}], "-", 
      RowBox[{
       SubscriptBox["a", "z"], 
       SubscriptBox["s", "23"]}]}], ")"}]}]}]],
  CellChangeTimes->{{3.750590312889607*^9, 3.750590335004607*^9}, {
    3.750590392362607*^9, 3.7505904601096067`*^9}, {3.7506278846588154`*^9, 
    3.750627885860016*^9}, {3.750628035700817*^9, 3.750628035841217*^9}, {
    3.750628161577818*^9, 3.7506281704074173`*^9}, {3.750628216926618*^9, 
    3.7506283115874186`*^9}, {3.750628403207219*^9, 3.7506285135910196`*^9}, 
    3.75062857482102*^9},
  ExpressionUUID -> "34e706c0-fc45-4fee-a126-89599687faec"],
 "\n",
 Cell[BoxData[
  StyleBox[
   RowBox[{
    SubscriptBox["\[Theta]", "4"], "=", 
    RowBox[{"Atan2", 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SubscriptBox["s", "4"], 
        SubscriptBox["s", "5"]}], ",", 
       RowBox[{
        SubscriptBox["c", "4"], 
        SubscriptBox["s", "5"]}]}], ")"}]}]}],
   Background->RGBColor[1, 0.5, 0]]],
  CellChangeTimes->{{3.750590312889607*^9, 3.750590335004607*^9}, {
    3.750590392362607*^9, 3.7505904601096067`*^9}, {3.7506278846588154`*^9, 
    3.750627885860016*^9}, {3.750628035700817*^9, 3.750628035841217*^9}, {
    3.750628161577818*^9, 3.7506281704074173`*^9}, {3.750628216926618*^9, 
    3.7506283115874186`*^9}, {3.750628403207219*^9, 3.7506285135910196`*^9}, 
    3.75062857482102*^9},
  ExpressionUUID -> "34e706c0-fc45-4fee-a126-89599687faec"]
}], "Text",
 CellChangeTimes->{{3.8017070374722214`*^9, 3.8017070543919606`*^9}, {
   3.8017074633894777`*^9, 3.8017074987763357`*^9}, {3.8017075349425635`*^9, 
   3.8017075854115596`*^9}, {3.801707638157466*^9, 3.801707642484895*^9}, {
   3.801707904405363*^9, 3.8017079357624836`*^9}, {3.8017148607671223`*^9, 
   3.801715005673502*^9}, 3.801715049773538*^9}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"IT34", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], ",", "0", ",", 
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "4"], "]"}]}], ",", 
        RowBox[{
         RowBox[{"-", 
          SubscriptBox["a", "3"]}], "*", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "4"], "]"}]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "4"], "]"}]}], ",", "0", ",", 
        RowBox[{"-", 
         RowBox[{"Cos", "[", 
          SubscriptBox["\[Theta]", "4"], "]"}]}], ",", 
        RowBox[{"a3", "*", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "4"], "]"}]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "1", ",", "0", ",", 
        RowBox[{"-", 
         SubscriptBox["d", "4"]}]}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ")"}], "//",
   "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.8017150709179754`*^9, 3.801715113990656*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "4"], "]"}], "0", 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}]}], 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
       SubscriptBox["a", "3"]}]},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}]}], "0", 
      RowBox[{"-", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}]}], 
      RowBox[{"a3", " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "4"], "]"}]}]},
     {"0", "1", "0", 
      RowBox[{"-", 
       SubscriptBox["d", "4"]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801715067316608*^9, 3.8017151166215825`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"IT34", ".", "IT23", ".", "IT12", ".", "IT01", ".", "ITT"}], ")"}],
   "//", "MatrixForm"}]], "Input"],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", "a1"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["a", "3"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "z"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["n", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["o", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["a", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{"a3", " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
       RowBox[{"a1", " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["p", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "z"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{"a1", " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "-", 
       SubscriptBox["d", "4"], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "1"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "z"]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.8017151285865765`*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"T46", "=", 
    RowBox[{"T45", ".", "T56"}]}], ")"}], "//", "MatrixForm"}]], "Input"],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}], " ", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], "0"},
     {
      RowBox[{"Sin", "[", 
       SubscriptBox["\[Theta]", "6"], "]"}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "6"], "]"}], "0", "0"},
     {
      RowBox[{
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], 
      RowBox[{
       RowBox[{"-", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}]}], " ", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801715143249354*^9}]
}, Open  ]],

Cell[TextData[{
 "\:4ee5\:4e0a\:4e24\:4e2a\:77e9\:9635\:76f8\:7b49\:ff0c\:5373\:4e24\:8fb9\
\:ff081\:ff0c3\:ff09\:9879\:548c\:ff083\:ff0c3\:ff09\:9879\:76f8\:7b49\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["s", "5"], "=", 
   RowBox[{"-", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       SubscriptBox["a", "x"], 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SubscriptBox["c", "1"], 
          SubscriptBox["c", "4"], 
          SubscriptBox["c", "23"]}], "+", 
         RowBox[{
          SubscriptBox["s", "4"], 
          SubscriptBox["s", "1"]}]}], ")"}]}], "+", 
      RowBox[{
       SubscriptBox["a", "y"], 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SubscriptBox["s", "1"], 
          SubscriptBox["c", "4"], 
          SubscriptBox["c", "23"]}], "-", 
         RowBox[{
          SubscriptBox["s", "4"], 
          SubscriptBox["c", "1"]}]}], ")"}]}], "-", 
      RowBox[{
       SubscriptBox["a", "z"], 
       SubscriptBox["c", "4"], 
       SubscriptBox["s", "23"]}]}], ")"}]}]}]],
  CellChangeTimes->{{3.750629013121623*^9, 3.7506290676436234`*^9}, {
   3.750629199650824*^9, 3.750629339506825*^9}, {3.7506294073668256`*^9, 
   3.7506294997188263`*^9}},
  ExpressionUUID -> "fba69237-ac02-45f7-895f-ec1b23bdb555"],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["c", "5"], "=", 
   RowBox[{
    RowBox[{
     RowBox[{"-", 
      SubscriptBox["a", "x"]}], 
     SubscriptBox["c", "1"], 
     SubscriptBox["s", "23"]}], "-", 
    RowBox[{
     SubscriptBox["a", "y"], 
     SubscriptBox["s", "1"], 
     SubscriptBox["s", "23"]}], "-", 
    RowBox[{
     SubscriptBox["a", "z"], 
     SubscriptBox["c", "23"]}]}]}]],
  CellChangeTimes->{{3.750629013121623*^9, 3.7506290676436234`*^9}, {
   3.750629199650824*^9, 3.750629339506825*^9}, {3.7506294073668256`*^9, 
   3.7506294997188263`*^9}},
  ExpressionUUID -> "fba69237-ac02-45f7-895f-ec1b23bdb555"],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["\[Theta]", "5"], "=", 
   RowBox[{"Atan2", 
    RowBox[{"(", 
     RowBox[{
      SubscriptBox["s", "5"], ",", 
      SubscriptBox["c", "5"]}], ")"}]}]}]],
  CellChangeTimes->{{3.750629013121623*^9, 3.7506290676436234`*^9}, {
   3.750629199650824*^9, 3.750629339506825*^9}, {3.7506294073668256`*^9, 
   3.7506294997188263`*^9}},
  Background->RGBColor[1, 0.5, 0],
  ExpressionUUID -> "fba69237-ac02-45f7-895f-ec1b23bdb555"]
}], "Text",
 CellChangeTimes->{{3.8017152558212595`*^9, 3.8017153072427263`*^9}}],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", 
   RowBox[{"IT45", "=", 
    RowBox[{"{", 
     RowBox[{
      RowBox[{"{", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], ",", "0", ",", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{
        RowBox[{"-", 
         RowBox[{"Sin", "[", 
          SubscriptBox["\[Theta]", "5"], "]"}]}], ",", "0", ",", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", 
        RowBox[{"-", "1"}], ",", "0", ",", "0"}], "}"}], ",", 
      RowBox[{"{", 
       RowBox[{"0", ",", "0", ",", "0", ",", "1"}], "}"}]}], "}"}]}], ")"}], "//",
   "MatrixForm"}]], "Input",
 CellChangeTimes->{{3.8017153689186954`*^9, 3.801715372946958*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], "0", 
      RowBox[{"Sin", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], "0"},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "5"], "]"}]}], "0", 
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "5"], "]"}], "0"},
     {"0", 
      RowBox[{"-", "1"}], "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801715375413325*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{
   RowBox[{"(", 
    RowBox[{
    "IT45", ".", "IT34", ".", "IT23", ".", "IT12", ".", "IT01", ".", "ITT"}], 
    ")"}], "//", "MatrixForm"}], "//", "Simplify"}]], "Input",
 CellChangeTimes->{{3.801715386464805*^9, 3.8017153896382713`*^9}}],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["n", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["o", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["a", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", "a1"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "1"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        SubscriptBox["a", "3"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        SubscriptBox["d", "4"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Sin", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "3"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["p", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "2"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "2"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["p", "z"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["n", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["o", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["a", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{"a1", " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "3"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "1"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        SubscriptBox["a", "3"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "5"], "]"}], " ", 
        SubscriptBox["d", "4"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Sin", "[", 
             SubscriptBox["\[Theta]", "1"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}]}], "+", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
             RowBox[{
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"(", 
               RowBox[{
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}], "-", 
                RowBox[{
                 RowBox[{"Cos", "[", 
                  SubscriptBox["\[Theta]", "4"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "2"], "]"}], " ", 
                 RowBox[{"Sin", "[", 
                  SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], 
            ")"}]}]}], ")"}], " ", 
        SubscriptBox["p", "y"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "2"], "]"}], " ", 
           RowBox[{"(", 
            RowBox[{
             RowBox[{
              RowBox[{"-", 
               RowBox[{"Cos", "[", 
                SubscriptBox["\[Theta]", "3"], "]"}]}], " ", 
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}], "+", 
             RowBox[{
              RowBox[{"Cos", "[", 
               SubscriptBox["\[Theta]", "4"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "3"], "]"}], " ", 
              RowBox[{"Sin", "[", 
               SubscriptBox["\[Theta]", "5"], "]"}]}]}], ")"}]}]}], ")"}], 
        " ", 
        SubscriptBox["p", "z"]}]}]},
     {
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["n", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["n", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["o", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["o", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["a", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["a", "z"]}]}], 
      RowBox[{
       RowBox[{
        RowBox[{"-", "a3"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "-", 
       RowBox[{"a1", " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
       RowBox[{
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "2"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["a", "1"]}], "-", 
       RowBox[{
        RowBox[{"Cos", "[", 
         SubscriptBox["\[Theta]", "3"], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["a", "2"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"-", 
            RowBox[{"Cos", "[", 
             SubscriptBox["\[Theta]", "4"], "]"}]}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "x"]}], "+", 
       RowBox[{
        RowBox[{"(", 
         RowBox[{
          RowBox[{
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Cos", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}], "+", 
          RowBox[{
           RowBox[{"Cos", "[", 
            RowBox[{
             SubscriptBox["\[Theta]", "2"], "+", 
             SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "1"], "]"}], " ", 
           RowBox[{"Sin", "[", 
            SubscriptBox["\[Theta]", "4"], "]"}]}]}], ")"}], " ", 
        SubscriptBox["p", "y"]}], "-", 
       RowBox[{
        RowBox[{"Sin", "[", 
         RowBox[{
          SubscriptBox["\[Theta]", "2"], "+", 
          SubscriptBox["\[Theta]", "3"]}], "]"}], " ", 
        RowBox[{"Sin", "[", 
         SubscriptBox["\[Theta]", "4"], "]"}], " ", 
        SubscriptBox["p", "z"]}]}]},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.801715418633709*^9}]
}, Open  ]],

Cell[CellGroupData[{

Cell[BoxData[
 RowBox[{
  RowBox[{"(", "T56", ")"}], "//", "MatrixForm"}]], "Input"],

Cell[BoxData[
 TagBox[
  RowBox[{"(", "\[NoBreak]", GridBox[{
     {
      RowBox[{"Cos", "[", 
       SubscriptBox["\[Theta]", "6"], "]"}], 
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], "0", "0"},
     {"0", "0", "1", "0"},
     {
      RowBox[{"-", 
       RowBox[{"Sin", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], 
      RowBox[{"-", 
       RowBox[{"Cos", "[", 
        SubscriptBox["\[Theta]", "6"], "]"}]}], "0", "0"},
     {"0", "0", "0", "1"}
    },
    GridBoxAlignment->{
     "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, 
      "RowsIndexed" -> {}},
    GridBoxSpacings->{"Columns" -> {
        Offset[0.27999999999999997`], {
         Offset[0.7]}, 
        Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
        Offset[0.2], {
         Offset[0.4]}, 
        Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
  Function[BoxForm`e$, 
   MatrixForm[BoxForm`e$]]]], "Output",
 CellChangeTimes->{3.8017154318802757`*^9}]
}, Open  ]],

Cell[TextData[{
 "(1,1)\:4e0e(3,1)\:5143\:7d20\:76f8\:7b49\:ff0c\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["c", "6"], "=", 
   RowBox[{
    RowBox[{
     SubscriptBox["c", "5"], 
     SubscriptBox["s", "4"], 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SubscriptBox["n", "x"], 
        SubscriptBox["s", "1"]}], "-", 
       RowBox[{
        SubscriptBox["n", "y"], 
        SubscriptBox["c", "1"]}]}], ")"}]}], "+", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SubscriptBox["c", "4"], 
        SubscriptBox["c", "5"], 
        SubscriptBox["s", "3"]}], "+", 
       RowBox[{
        SubscriptBox["c", "3"], 
        SubscriptBox["s", "5"]}]}], ")"}], 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        RowBox[{"-", 
         SubscriptBox["n", "x"]}], 
        SubscriptBox["c", "1"], 
        SubscriptBox["s", "2"]}], "-", 
       RowBox[{
        SubscriptBox["n", "y"], 
        SubscriptBox["s", "1"], 
        SubscriptBox["s", "2"]}], "-", 
       RowBox[{
        SubscriptBox["n", "z"], 
        SubscriptBox["c", "z"]}]}], ")"}]}], "+", 
    RowBox[{
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SubscriptBox["c", "3"], 
        SubscriptBox["c", "4"], 
        SubscriptBox["c", "5"]}], "-", 
       RowBox[{
        SubscriptBox["s", "3"], 
        SubscriptBox["s", "5"]}]}], ")"}], 
     RowBox[{"(", 
      RowBox[{
       RowBox[{
        SubscriptBox["n", "x"], 
        SubscriptBox["c", "1"], 
        SubscriptBox["c", "2"]}], "+", 
       RowBox[{
        SubscriptBox["n", "y"], 
        SubscriptBox["s", "1"], 
        SubscriptBox["c", "2"]}], "-", 
       RowBox[{
        SubscriptBox["n", "z"], 
        SubscriptBox["s", "2"]}]}], ")"}]}]}]}]],
  CellChangeTimes->{{3.750629791862028*^9, 3.750629822375628*^9}, {
   3.7506303807796316`*^9, 3.750630413961832*^9}, {3.750630456456232*^9, 
   3.750630456830632*^9}, {3.750630550103033*^9, 3.750630608619633*^9}, {
   3.750631578127239*^9, 3.75063163566004*^9}, {3.750631870310241*^9, 
   3.750632188505443*^9}, {3.7506323921810446`*^9, 3.7506325186834455`*^9}, {
   3.7506329361580477`*^9, 3.7506329748928485`*^9}, {3.7506330129412484`*^9, 
   3.7506330186040487`*^9}, {3.750633068010249*^9, 3.750633153279849*^9}},
  ExpressionUUID -> "cfc37e4d-c420-4ea7-9f1e-38c9dc6c7577"],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["s", "6"], "=", 
   RowBox[{"-", 
    RowBox[{"(", 
     RowBox[{
      RowBox[{
       SubscriptBox["n", "x"], 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          RowBox[{"-", 
           SubscriptBox["c", "4"]}], 
          SubscriptBox["s", "1"]}], "+", 
         RowBox[{
          SubscriptBox["c", "1"], 
          SubscriptBox["s", "4"], 
          SubscriptBox["c", "23"]}]}], ")"}]}], "+", 
      RowBox[{
       SubscriptBox["n", "y"], 
       RowBox[{"(", 
        RowBox[{
         RowBox[{
          SubscriptBox["c", "4"], 
          SubscriptBox["c", "1"]}], "+", 
         RowBox[{
          SubscriptBox["s", "1"], 
          SubscriptBox["s", "4"], 
          SubscriptBox["c", "23"]}]}], ")"}]}], "-", 
      RowBox[{
       SubscriptBox["n", "z"], 
       SubscriptBox["s", "4"], 
       SubscriptBox["s", "23"]}]}], ")"}]}]}]],
  CellChangeTimes->{{3.750629791862028*^9, 3.750629822375628*^9}, {
   3.7506303807796316`*^9, 3.750630413961832*^9}, {3.750630456456232*^9, 
   3.750630456830632*^9}, {3.750630550103033*^9, 3.750630608619633*^9}, {
   3.750631578127239*^9, 3.75063163566004*^9}, {3.750631870310241*^9, 
   3.750632188505443*^9}, {3.7506323921810446`*^9, 3.7506325186834455`*^9}, {
   3.7506329361580477`*^9, 3.7506329748928485`*^9}, {3.7506330129412484`*^9, 
   3.7506330186040487`*^9}, {3.750633068010249*^9, 3.750633153279849*^9}},
  ExpressionUUID -> "cfc37e4d-c420-4ea7-9f1e-38c9dc6c7577"],
 "\n",
 Cell[BoxData[
  RowBox[{
   SubscriptBox["\[Theta]", "6"], "=", 
   RowBox[{"Atan2", 
    RowBox[{"(", 
     RowBox[{
      SubscriptBox["s", "6"], ",", 
      SubscriptBox["c", "6"]}], ")"}]}]}]],
  CellChangeTimes->{{3.750629791862028*^9, 3.750629822375628*^9}, {
   3.7506303807796316`*^9, 3.750630413961832*^9}, {3.750630456456232*^9, 
   3.750630456830632*^9}, {3.750630550103033*^9, 3.750630608619633*^9}, {
   3.750631578127239*^9, 3.75063163566004*^9}, {3.750631870310241*^9, 
   3.750632188505443*^9}, {3.7506323921810446`*^9, 3.7506325186834455`*^9}, {
   3.7506329361580477`*^9, 3.7506329748928485`*^9}, {3.7506330129412484`*^9, 
   3.7506330186040487`*^9}, {3.750633068010249*^9, 3.750633153279849*^9}},
  Background->RGBColor[1, 0.5, 0],
  ExpressionUUID -> "cfc37e4d-c420-4ea7-9f1e-38c9dc6c7577"]
}], "Text",
 CellChangeTimes->{{3.8017155213798666`*^9, 3.8017155436383247`*^9}, {
  3.801715573807646*^9, 3.801715589108694*^9}}]
}, Open  ]]
}, Open  ]]
},
WindowSize->{1920, 997},
WindowMargins->{{-8, Automatic}, {Automatic, -8}},
Magnification:>1.5 Inherited,
FrontEndVersion->"11.0 for Microsoft Windows (64-bit) (2016\:5e748\:670817\
\:65e5)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)

(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[CellGroupData[{
Cell[580, 22, 1596, 43, 142, "Input"],
Cell[2179, 67, 1069, 30, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[3285, 102, 1658, 46, 142, "Input"],
Cell[4946, 150, 1146, 33, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[6129, 188, 1599, 44, 142, "Input"],
Cell[7731, 234, 1098, 31, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[8866, 270, 1689, 47, 142, "Input"],
Cell[10558, 319, 1170, 34, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[11765, 358, 1603, 44, 142, "Input"],
Cell[13371, 404, 1086, 31, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[14494, 440, 1628, 45, 142, "Input"],
Cell[16125, 487, 1118, 32, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[17280, 524, 155, 3, 113, "Title"],
Cell[CellGroupData[{
Cell[17460, 531, 289, 8, 68, "Input"],
Cell[17752, 541, 28066, 745, 142, "Output"]
}, Open  ]]
}, Open  ]],
Cell[CellGroupData[{
Cell[45867, 1292, 234, 4, 113, "Title"],
Cell[CellGroupData[{
Cell[46126, 1300, 200, 3, 79, "Chapter"],
Cell[CellGroupData[{
Cell[46351, 1307, 818, 26, 70, "Input"],
Cell[47172, 1335, 1061, 33, 142, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[48270, 1373, 811, 25, 68, "Input"],
Cell[49084, 1400, 1070, 30, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[50191, 1435, 1407, 37, 142, "Input"],
Cell[51601, 1474, 8661, 247, 142, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[60299, 1726, 990, 27, 142, "Input"],
Cell[61292, 1755, 3337, 103, 147, "Output"]
}, Open  ]],
Cell[64644, 1861, 8999, 218, 371, "Text"],
Cell[CellGroupData[{
Cell[73668, 2083, 1178, 34, 68, "Input"],
Cell[74849, 2119, 1318, 39, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[76204, 2163, 1153, 33, 68, "Input"],
Cell[77360, 2198, 1270, 38, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[78667, 2241, 244, 6, 68, "Input"],
Cell[78914, 2249, 9500, 279, 151, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[88451, 2533, 152, 5, 68, "Input"],
Cell[88606, 2540, 3453, 101, 142, "Output"]
}, Open  ]],
Cell[92074, 2644, 11014, 334, 344, "Text"],
Cell[CellGroupData[{
Cell[103113, 2982, 1232, 36, 68, "Input"],
Cell[104348, 3020, 1365, 41, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[105750, 3066, 153, 4, 68, "Input"],
Cell[105906, 3072, 33442, 910, 151, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[139385, 3987, 137, 4, 68, "Input"],
Cell[139525, 3993, 1715, 51, 142, "Output"]
}, Open  ]],
Cell[141255, 4047, 2471, 75, 123, "Text"],
Cell[CellGroupData[{
Cell[143751, 4126, 889, 25, 68, "Input"],
Cell[144643, 4153, 1019, 30, 141, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[145699, 4188, 274, 7, 68, "Input"],
Cell[145976, 4197, 57891, 1512, 151, "Output"]
}, Open  ]],
Cell[CellGroupData[{
Cell[203904, 5714, 84, 2, 68, "Input"],
Cell[203991, 5718, 1047, 31, 141, "Output"]
}, Open  ]],
Cell[205053, 5752, 4760, 134, 123, "Text"]
}, Open  ]]
}, Open  ]]
}
]
*)

